package chw412;

import java.util.ArrayList;
import java.util.LinkedList;
import java.util.Queue;

import chw412.KnownAgent.agent_state;

import com.sun.xml.internal.bind.v2.runtime.unmarshaller.XsiNilLoader.Array;

import agent.Action;
import agent.Percept;
import agent.Agent;
import pacworld.*;

import java.util.*;

import agent.Action;

//this is the state of the map as far as I known.
enum map_state {
	Package, Clear, Unknown
};

/**
 * An abstract software agent class. The agent must be managed by the
 * Environment class, which calls its see() and selectAction() methods.
 */
public class PacAgent extends Agent {

	protected String id;
	protected int x = -1;
	protected int y = -1;

	protected Action next_action = null;
	protected PacPercept percept = null;

	// Other agents I know
	protected ArrayList<KnownAgent> agt_info = null;

	// Packages I know
	protected ArrayList<KnownPackage> pkg_info = null;

	// the map representing the world.
	protected map_state[][] map = null;
	protected int map_size = -1;

	// this is ID of the intent package I were asked to achieve and deliver
	// based on coordinator's command.
	protected int global_intention = -1;

	// this is my own intention
	protected Location local_intention = null;

	//my current goal
	protected Location goal = null;
	
	//my current delivery destination, if any
	protected Location dest = null;
	
	// this is the plan to get close to my goal.
	protected ArrayList<Integer> plan = null;

	// this is used to decide who is the coordinator;
	// coordinator will have value true, while others will have value false.
	protected boolean coordinator = false;

	// this is used ONLY by coordinator to decide whether it is necessary to do
	// a global coordination.
	// Events like New package(s) detected, Someone's picking up a package or
	// dropping off a package will trigger global coordination.
	protected boolean need_coordinate = false;

	// move history records the direction that I move in each step.
	protected ArrayList<Integer> move_history = null;

	// the last action executed
	protected Action last_action = null;

	// the package dropped in the last turn, if last turn is not Drop-off
	// action, dropped_package == -1
	protected int dropped_package = -1;

	// the package picked up in the last turn, if last turn is not picked-up
	// action, picked_up_package = -1
	protected int picked_up_package = -1;

	protected boolean starter = true; // after introduction, it will be set as
										// false;

	public enum state_t {
		INTENT, // the agent is pursuing some certain target package
		CARRY, // the agent is carrying some package
		EXPLORE, // the agent is exploring the unknown area in the world
		PICKUP,
		DROPOFF,
		IDLE // , by default, agents are idling.
	};

	private state_t state;

	/**
	 * All subclass should create a one-arg constructor that takes an integer
	 * and invokes this constructor. This constructor would be used in
	 * multi-agent environments in order to distinguish the different agents.
	 */
	public PacAgent(int id) {
		this.id = Integer.toString(id);
		agt_info = new ArrayList<KnownAgent>();
		pkg_info = new ArrayList<KnownPackage>();
		move_history = new ArrayList<Integer>();
		plan = new ArrayList<Integer>();
		state = state_t.IDLE;
	}

	/**
	 * @param agent_id
	 *            the agent ID
	 * @param move_count
	 *            the number of moves since last update
	 * @param str
	 *            the move string
	 */
	void update_map(int agent_id, int move_count, String str) {
		assert (str.length() >= 3);
		ArrayList<Integer> move = new ArrayList<Integer>();

		int[] dir = new int[4];
		for (int i = 3; i < 3 + move_count / 4; ++i) {
			int ch = (int) str.charAt(i);
			for (int j = 3; j >= 0; j--) {
				dir[j] = ch & 3;
				ch >>= 2;
			}
			for (int j = 0; j <= 3; ++j) {
				move.add(dir[j]);
			}
		}

		for (int i = 3 + move_count / 4; i < str.length(); ++i) {
			int d = move_count % 4;
			assert (d > 0);
			d = 4 - d;
			d <<= 1;
			int ch = (int) str.charAt(i);
			ch >>= d;
			for (int j = move_count % 4; j >= 0; j--) {
				dir[j] = ch & 3;
				ch >>= 2;
			}
			for (int j = 0; j <= move_count % 4; ++j) {
				move.add(dir[j]);
			}
		}

		int index = Collections.binarySearch(agt_info,
				new KnownAgent(Integer.toString(agent_id), -1, -1));
		assert (index >= 0);
		int ax = agt_info.get(index).getX();
		int ay = agt_info.get(index).getY();
		for (Integer m : move) {
			ax += Direction.DELTA_X[m.intValue()];
			ay += Direction.DELTA_Y[m.intValue()];
			for (int i = ax - PacPercept.VIS_RADIUS; i <= ax
					+ PacPercept.VIS_RADIUS; ++i) {
				for (int j = ay - PacPercept.VIS_RADIUS; j <= ay
						+ PacPercept.VIS_RADIUS; ++j) {
					if (i >= 0 && i < map_size && j >= 0 && j < map_size
							&& map[i][j] == map_state.Unknown) {
						map[i][j] = map_state.Clear;
					}
				}
			}
		}

		agt_info.get(index).update_loc(ax, ay);
	}

	/**
	 * @param ka
	 *            the agent that just joined our team
	 */
	void update_map(KnownAgent ka) {
		int ax = ka.getX();
		int ay = ka.getY();
		for (int i = ax - PacPercept.VIS_RADIUS; i <= ax
				+ PacPercept.VIS_RADIUS; ++i) {
			for (int j = ay - PacPercept.VIS_RADIUS; j <= ay
					+ PacPercept.VIS_RADIUS; ++j) {
				if (in_map_range(new Point(i, j, null))
						&& map[i][j] == map_state.Unknown) {
					map[i][j] = map_state.Clear;
				}
			}
		}
	}

	/**
	 * Parse the received messages at each turn
	 */
	boolean parse_messages(String[] msgs) {
		int pid, x, y, z, w, index;
		int agent_id;
		int move_count;
		String idd, h, move_history;
		KnownAgent ka;
		KnownPackage kp;

		for (String str : msgs) {
			switch (str.charAt(0)) {
			case 'A': // messages that indicate that new agent joins our team.
				idd = str.substring(str.indexOf('A') + 1, str.indexOf('X'));
				// If I received update info from others, update my map.
				// Otherwise, don't since it's older.
				if (Integer.parseInt(idd) == Integer.parseInt(id))
					break;

				x = Integer.parseInt(str.substring(str.indexOf('X') + 1,
						str.indexOf('Y')));
				y = Integer.parseInt(str.substring(str.indexOf('Y') + 1,
						str.length()));
				ka = new KnownAgent(idd, x, y);
				agt_info.add(ka);
				Collections.sort(agt_info);
				update_map(ka);
				break;
			case 'P': // messages that indicate that new packages identified by
						// some team member.
				String pkgs = str.substring(0, str.indexOf('A'));
				String agt_loc = str.substring(str.indexOf('A'),
						str.indexOf('M'));
				move_history = str.substring(str.indexOf('M'), str.length());

				// parse agent location info
				idd = agt_loc.substring(agt_loc.indexOf('A') + 1,
						agt_loc.indexOf('X'));
				if (Integer.parseInt(idd) == Integer.parseInt(id))
					break;

				x = Integer.parseInt(agt_loc.substring(
						agt_loc.indexOf('X') + 1, agt_loc.indexOf('Y')));
				y = Integer.parseInt(agt_loc.substring(
						agt_loc.indexOf('Y') + 1, agt_loc.length()));
				ka = new KnownAgent(idd, x, y);
				index = Collections.binarySearch(agt_info, ka);
				assert (index >= 0);
				agt_info.get(index).update_loc(Integer.valueOf(x),
						Integer.valueOf(y));

				// parse package info
				String[] pkg_strs = pkgs.split("P");
				for (String pkg_str : pkg_strs) {
					if (pkg_str.length() > 0) {
						pid = Integer.parseInt(pkg_str.substring(0,
								pkg_str.indexOf('X')));
						x = Integer
								.parseInt(pkg_str.substring(
										pkg_str.indexOf('X') + 1,
										pkg_str.indexOf('Y')));
						y = Integer
								.parseInt(pkg_str.substring(
										pkg_str.indexOf('Y') + 1,
										pkg_str.indexOf('Z')));
						z = Integer
								.parseInt(pkg_str.substring(
										pkg_str.indexOf('Z') + 1,
										pkg_str.indexOf('W')));
						w = Integer
								.parseInt(pkg_str.substring(
										pkg_str.indexOf('W') + 1,
										pkg_str.indexOf('H')));
						h = pkg_str.substring(pkg_str.indexOf('H') + 1,
								pkg_str.length());
						kp = new KnownPackage(pid, x, y, z, w,
								((h == "Y") ? true : false));
						index = Collections.binarySearch(pkg_info, kp);
						if (index >= 0) {
							; // overlook
						} else {
							pkg_info.add(kp);
							Collections.sort(pkg_info);
							if (!kp.isHeld())
								map[kp.getX()][kp.getY()] = map_state.Package;
						}
					}
				}

				// parse agent move history info
				if (move_history.length() > 3) {
					agent_id = (int) move_history.charAt(1);
					move_count = (int) move_history.charAt(2);
					update_map(agent_id, move_count, move_history);
				}

				// remember that new packages detected triggers global
				// coordination.
				need_coordinate = true;
				break;
			case 'D': // messages indicate that some agent has dropped a package
						// into its destination.
				// parse agent location
				idd = str.substring(str.lastIndexOf('A') + 1,
						str.lastIndexOf('X'));
				if (Integer.parseInt(idd) == Integer.parseInt(id))
					break;

				x = Integer.parseInt(str.substring(str.lastIndexOf('X') + 1,
						str.lastIndexOf('Y')));
				y = Integer.parseInt(str.substring(str.lastIndexOf('Y') + 1,
						str.lastIndexOf('M')));
				ka = new KnownAgent(idd, x, y);
				index = Collections.binarySearch(agt_info, ka);
				agt_info.get(index).update_loc(Integer.valueOf(x),
						Integer.valueOf(y));
				agt_info.get(index).update_state(agent_state.IDLE);

				// parse drop package id
				String dppk = str.substring(str.indexOf('D') + 1,
						str.lastIndexOf('A'));
				String[] dpk = dppk.split("P");
				int dp = Integer.parseInt(dpk[0]);
				index = Collections.binarySearch(pkg_info, new KnownPackage(dp,
						-1, -1, -1, -1, false));
				if (index >= 0) {
					// mark the position of the deleted package in the map as
					// clear
					map[pkg_info.get(index).getX()][pkg_info.get(index).getY()] = map_state.Clear;
					pkg_info.remove(pkg_info.get(index));
				}
				if (dpk.length == 2) { // not successfully delivered
					pid = Integer.parseInt(dpk[1].substring(0,
							dpk[1].indexOf('X')));
					x = Integer.parseInt(dpk[1].substring(
							dpk[1].indexOf('X') + 1, dpk[1].indexOf('Y')));
					y = Integer.parseInt(dpk[1].substring(
							dpk[1].indexOf('Y') + 1, dpk[1].indexOf('Z')));
					z = Integer.parseInt(dpk[1].substring(
							dpk[1].indexOf('Z') + 1, dpk[1].indexOf('W')));
					w = Integer.parseInt(dpk[1].substring(
							dpk[1].indexOf('W') + 1, dpk[1].indexOf('H')));
					h = dpk[1].substring(dpk[1].indexOf('H') + 1,
							dpk[1].length());
					kp = new KnownPackage(pid, x, y, z, w, ((h == "Y") ? true
							: false));
					pkg_info.add(kp);
					map[kp.getX()][kp.getY()] = map_state.Package;
				}

				// parse agent move history
				move_history = str.substring(str.indexOf('M'), str.length());
				if (move_history.length() > 3) {
					agent_id = (int) move_history.charAt(1);
					move_count = (int) move_history.charAt(2);
					update_map(agent_id, move_count, move_history);
				}

				// remember that coordinator needs to do a coordination.
				need_coordinate = true;
				break;
			case 'U': // messages indicate that some agent has picked up a
						// package.
				// parse agent location
				idd = str.substring(str.indexOf('A') + 1, str.indexOf('X'));
				if (Integer.parseInt(idd) == Integer.parseInt(id))
					break;

				x = Integer.parseInt(str.substring(str.indexOf('X') + 1,
						str.indexOf('Y')));
				y = Integer.parseInt(str.substring(str.indexOf('Y') + 1,
						str.indexOf('M')));
				ka = new KnownAgent(idd, x, y);
				index = Collections.binarySearch(agt_info, ka);
				if (index >= 0) {
					agt_info.get(index).update_loc(Integer.valueOf(x),
							Integer.valueOf(y));
					agt_info.get(index).update_state(agent_state.CARRY);
				}

				// parse picked-up package id
				int up = Integer.parseInt(str.substring(str.indexOf('U') + 1,
						str.indexOf('A')));
				index = Collections.binarySearch(pkg_info, new KnownPackage(up,
						-1, -1, -1, -1, false));
				if (index >= 0) {
					pkg_info.get(index).setheld();
					// since someone lift this package, the position that this
					// package originally occupy will be marked as clear.
					map[pkg_info.get(index).getX()][pkg_info.get(index).getY()] = map_state.Clear;
				}

				// parse move history
				move_history = str.substring(str.indexOf('M'), str.length());
				if (move_history.length() > 3) {
					agent_id = (int) move_history.charAt(1);
					move_count = (int) move_history.charAt(2);
					update_map(agent_id, move_count, move_history);
				}

				need_coordinate = true;
				break;
			case 'M': // messages indicate some agent has move for some while
						// and wants to inform me its move history
						// so that I can compute the known area that it has
						// explored in the map.
				agent_id = (int) str.charAt(1);
				if (agent_id == Integer.parseInt(id))
					break;
				move_count = (int) str.charAt(2);
				update_map(agent_id, move_count, str);
				break;
			case 'C': // messages indicate the command of coordinator.
				// If I am now carrying a package, ignore this message
				// or else I get the coordinator's command.
				if (this.state != state_t.CARRY) {
					String[] cmds = str.split("A");
					for (String cmd : cmds) {
						if (cmd.length() <= 1)
							continue;
						idd = cmd.substring(0, cmd.indexOf('P'));
						if (Integer.parseInt(this.id) == Integer.parseInt(idd)) {
							String aim_pkg = cmd.substring(
									cmd.indexOf('P') + 1, cmd.length());
							if (aim_pkg.contains("E")) {
								global_intention = -1;
							} else {
								global_intention = Integer.parseInt(aim_pkg);
								assert (global_intention >= 0);
							}
						}
					}
				}
				break;
			case 'S':
				need_coordinate = true;
				break;
			default:
				break;
			}
		}
		return true;
	}

	String construct_move_history_message() {
		int i = 0;
		String str = "M";
		int ch = Integer.parseInt(id);
		str += (char) ch;
		ch = move_history.size();
		str += (char) ch;

		for (i = 0; i + 3 < move_history.size(); i = i + 4) {
			ch = 0;
			ch += move_history.get(i).intValue();
			ch <<= 2;
			ch += move_history.get(i + 1).intValue();
			ch <<= 2;
			ch += move_history.get(i + 2).intValue();
			ch <<= 2;
			ch += move_history.get(i + 3).intValue();
			str += (char) ch;
		}

		ch = 0;
		for (; i < move_history.size(); ++i) {
			ch <<= 2;
			ch += move_history.get(i).intValue();
		}
		str += (char) ch;

		return str;
	}

	int dist(Location l1, Location l2) {
		return Math.abs(l1.getX() - l2.getX())
				+ Math.abs(l1.getY() - l2.getY());
	}

	// Get some agent info from my belief database according to the agent id if
	// exists.
	KnownAgent get_agent_info(String iid) {
		int index = Collections.binarySearch(agt_info, new KnownAgent(iid, -1,
				-1));
		if (index >= 0)
			return agt_info.get(index);
		else
			return null;
	}

	// Get the package info from my belief database according to the package id
	// if exists.
	KnownPackage get_package_info(int iid) {
		int index = Collections.binarySearch(pkg_info, new KnownPackage(iid,
				-1, -1, -1, -1, false));
		if (index >= 0)
			return pkg_info.get(index);
		else
			return null;
	}

	// Match available agents with available packages according to global
	// optimization.
	Map<KnownAgent, KnownPackage> map_packages(
			ArrayList<KnownAgent> available_agts,
			ArrayList<KnownPackage> available_pkgs) {
		int[][] value = new int[available_agts.size()][available_pkgs.size()];
		for (int i = 0; i < available_agts.size(); ++i) {
			for (int j = 0; j < available_pkgs.size(); ++j) {
				KnownAgent aa = available_agts.get(i);
				KnownPackage kp = available_pkgs.get(j);
				int dist1 = dist(new Location(aa.getX(), aa.getY()),
						new Location(kp.getX(), kp.getY()));
				int dist2 = dist(new Location(kp.getX(), kp.getY()),
						new Location(kp.getDestX(), kp.getDestY()));
				value[i][j] = dist1 + dist2;
			}
		}

		Map<KnownAgent, KnownPackage> map = new HashMap<KnownAgent, KnownPackage>();
		int count = Math.min(available_agts.size(), available_pkgs.size());
		while (count > 0) {
			int min = Integer.MAX_VALUE;
			int mi = -1;
			int mj = -1;
			for (int i = 0; i < available_agts.size(); ++i) {
				for (int j = 0; j < available_pkgs.size(); ++j) {
					if (value[i][j] < min) {
						min = value[i][j];
						mi = i;
						mj = j;
					}
				}
			}
			map.put(available_agts.get(mi), available_pkgs.get(mj));
			for (int j = 0; j < available_pkgs.size(); ++j)
				value[mi][j] = Integer.MAX_VALUE;
			for (int i = 0; i < available_agts.size(); ++i)
				value[i][mj] = Integer.MAX_VALUE;
			--count;
		}
		for (KnownAgent aa : available_agts) {
			if (!map.containsKey(aa)) { // no pkg assigned, so we assign dummy
										// package.
				map.put(aa, new KnownPackage(-1, 0, 0, 0, 0, false));
			}
		}

		return map;
	}

	/*
	 * get the direction between two adjacent locations, from l1 to l2 If l1 and
	 * l2 are not adjacent, return -1
	 */
	int get_direction(Location l1, Location l2) {
		int delta_x = l2.getX() - l1.getX();
		int delta_y = l2.getY() - l1.getY();
		if (delta_x == 0 && delta_y == -1)
			return Direction.NORTH;
		else if (delta_x == 1 && delta_y == 0)
			return Direction.EAST;
		else if (delta_x == 0 && delta_y == 1)
			return Direction.SOUTH;
		else if (delta_x == -1 && delta_y == 0)
			return Direction.WEST;
		else
			return -1;
	}

	/*
	 * check if I can move according to the direction I am going to move, the
	 * direction to which I am holding my package, if any, and my current
	 * instant percept.
	 */

	boolean check_movable(Integer dir, Integer pkg_dir) {
		VisibleAgent[] vas = percept.getVisAgents();
		VisiblePackage[] vps = percept.getVisPackages();

		if (pkg_dir == -1) { // I do not hold any package
			Location mf = new Location(this.x + Direction.DELTA_X[dir], this.y
					+ Direction.DELTA_Y[dir]);
			if (!in_map_range(new Point(mf, null)))
				return false;
			for (VisibleAgent va : vas) {
				if (mf.getX() == va.getX() && mf.getY() == va.getY())
					return false;
			}
			for (VisiblePackage vp : vps) {
				if (mf.getX() == vp.getX() && mf.getY() == vp.getY())
					return false;
			}
			return true;
		} else { // I am holding a package in direction pkg_dir
			if (dir == (pkg_dir + 1) % 4 || dir == (pkg_dir + 3) % 4) {
				Location mf = new Location(this.x + Direction.DELTA_X[dir],
						this.y + Direction.DELTA_Y[dir]);
				Location pf = new Location(this.x + Direction.DELTA_X[pkg_dir]
						+ Direction.DELTA_X[dir], this.y
						+ Direction.DELTA_Y[pkg_dir] + Direction.DELTA_Y[dir]);
				if (!in_map_range(new Point(mf, null))
						|| !in_map_range(new Point(pf, null)))
					return false;
				for (VisibleAgent va : vas) {
					if (mf.getX() == va.getX() && mf.getY() == va.getY()
							|| pf.getX() == va.getX() && pf.getY() == va.getY())
						return false;
				}
				for (VisiblePackage vp : vps) {
					if (mf.getX() == vp.getX() && mf.getY() == vp.getY()
							|| pf.getX() == vp.getX() && pf.getY() == vp.getY())
						return false;
				}
				return true;
			} else if (dir == (pkg_dir + 2) % 4) {
				Location mf = new Location(this.x + Direction.DELTA_X[dir],
						this.y + Direction.DELTA_Y[dir]);
				if (!in_map_range(new Point(mf, null)))
					return false;
				for (VisibleAgent va : vas) {
					if (mf.getX() == va.getX() && mf.getY() == va.getY())
						return false;
				}
				for (VisiblePackage vp : vps) {
					if (mf.getX() == vp.getX() && mf.getY() == vp.getY())
						return false;
				}
				return true;
			} else if (dir == pkg_dir) {
				Location pf = new Location(this.x + 2 * Direction.DELTA_X[dir],
						this.y + 2 * Direction.DELTA_Y[dir]);
				if (!in_map_range(new Point(pf, null)))
					return false;
				for (VisibleAgent va : vas) {
					if (pf.getX() == va.getX() && pf.getY() == va.getY())
						return false;
				}
				for (VisiblePackage vp : vps) {
					if (pf.getX() == vp.getX() && pf.getY() == vp.getY())
						return false;
				}
				return true;
			} else {
				assert (false);
				return true;
			}
		}
	}

	/*
	 * calculate the path from me to point p. Given that I am holding a package
	 * on my dir side or -1 if none. If no such path exist, return null
	 */
	ArrayList<Integer> compute_path(Point p, int pkg_dir) {
		Astar astar = new Astar(map, p, new Point(this.x, this.y, null),
				pkg_dir);
		if (astar.astar_search()) { // we found a path from me to target point
			return astar.construct_path();
		}
		return null;
	}

	/*
	 * find a reachable destination that is closest to location dest. pkg_dir
	 * indicates the direction I am holding the package. If no package is held,
	 * pkg_dir = -1 return null if none exist.
	 */
	Location Find_Nearest_Unknown(Location cur) {
		boolean[][] visited = new boolean[map_size][map_size];
		for (int i = 0; i < map_size; ++i)
			for (int j = 0; j < map_size; ++j)
				visited[i][j] = false;

		Queue<Location> v1 = new LinkedList<Location>();
		v1.add(cur);
		visited[cur.getX()][cur.getY()] = true;

		while (!v1.isEmpty()) {
			Location t = v1.remove();
			if (map[t.getX()][t.getY()] == map_state.Unknown) {
				return t;
			}

			for (int dir = 0; dir <= 3; ++dir) {
				Location p = new Location(t.getX() + Direction.DELTA_X[dir],
						t.getY() + Direction.DELTA_Y[dir]);
				if (in_map_range(new Point(p, null))
						&& visited[p.getX()][p.getY()] == false) {
					v1.add(p);
					visited[p.getX()][p.getY()] = true;
				}
			}
		}

		return null;
	}

	Location Find_Nearest_Clear(Location dest) {
		boolean[][] visited = new boolean[map_size][map_size];
		for (int i = 0; i < map_size; ++i)
			for (int j = 0; j < map_size; ++j)
				visited[i][j] = false;

		Queue<Location> v1 = new LinkedList<Location>();
		v1.add(dest);
		visited[dest.getX()][dest.getY()] = true;

		while (!v1.isEmpty()) {
			Location t = v1.remove();
			if (map[t.getX()][t.getY()] == map_state.Clear) {
				return t;
			}

			for (int dir = 0; dir <= 3; ++dir) {
				Location p = new Location(t.getX() + Direction.DELTA_X[dir],
						t.getY() + Direction.DELTA_Y[dir]);
				if (in_map_range(new Point(p, null))
						&& visited[p.getX()][p.getY()] == false) {
					v1.add(p);
					visited[p.getX()][p.getY()] = true;
				}
			}
		}

		return null;
	}

	Location Find_Nearest_Package(Location cur) {
		boolean[][] visited = new boolean[map_size][map_size];
		for (int i = 0; i < map_size; ++i)
			for (int j = 0; j < map_size; ++j)
				visited[i][j] = false;

		Queue<Location> v1 = new LinkedList<Location>();
		v1.add(cur);
		visited[cur.getX()][cur.getY()] = true;

		while (!v1.isEmpty()) {
			Location t = v1.remove();
			if (map[t.getX()][t.getY()] == map_state.Package) {
				return t;
			}

			for (int dir = 0; dir <= 3; ++dir) {
				Location p = new Location(t.getX() + Direction.DELTA_X[dir],
						t.getY() + Direction.DELTA_Y[dir]);
				if (in_map_range(new Point(p, null))
						&& visited[p.getX()][p.getY()] == false) {
					v1.add(p);
					visited[p.getX()][p.getY()] = true;
				}
			}
		}

		return null;
	}

	int Package_ID_At_Loc(Location l) {
		for (int i = 0; i < pkg_info.size(); ++i) {
			if (pkg_info.get(i).getX() == l.getX()
					&& pkg_info.get(i).getY() == l.getY())
				return pkg_info.get(i).getId();
		}
		return -1;
	}

	int round = 0;

	/**
	 * Provide a Percept to the agent. This function is called by the
	 * environment at the beginning of each agent's turn. If the agent has
	 * internal state, this method should also update it.
	 */
	public void see(Percept p) {
		System.out.println("-------------------------");
		System.out.println("-------------------------");
		System.out.println("Round: " + round);
		round++;

		percept = (PacPercept) p;
		percept.printPercept();

		// First time looking at something, initialize map and introduce
		// oneself.
		if (map == null) {
			// initialize map
			map_size = percept.getWorldSize();
			map = new map_state[map_size][map_size];
			for (int i = 0; i < map_size; ++i)
				for (int j = 0; j < map_size; ++j)
					map[i][j] = map_state.Unknown;
		}

		// parse the message that I received
		if (parse_messages(percept.getMessages()) == false)
			return;

		// update info according to the agents I see
		for (VisibleAgent va : percept.getVisAgents()) {
			// mark what other agents can see as clear
			update_map(new KnownAgent(va));
			if (va.getId() == id) {
				this.x = va.getX();
				this.y = va.getY();
			}
		}

		// this is me
		KnownAgent me = new KnownAgent(id, this.x, this.y);
		// this is my current location
		Location cur = new Location(this.x, this.y);
		// debug message
		System.out.println("I am Agent " + this.id + " at (" + this.x + ","
				+ this.y + ")");

		// introduce myself to others
		if (starter) {
			starter = false;
			agt_info.add(me);
			Collections.sort(agt_info);
			update_map(me);

			String msg = "";
			msg = "A" + id + "X" + x + "Y" + y; // tell others who I am and
												// where I am.
			next_action = new Say(msg);
			return;
		}

		// identify new packages if any and broadcast to others
		String msg = "";
		for (VisiblePackage vp : percept.getVisPackages()) {
			if (get_package_info(vp.getId()) == null) { // this is a new package
														// and no one has ever
														// broadcast it.
				KnownPackage kp = new KnownPackage(vp);
				pkg_info.add(kp);
				Collections.sort(pkg_info);
				if (!kp.isHeld())
					map[kp.getX()][kp.getY()] = map_state.Package;
				msg += "P" + kp.getId() + "X" + kp.getX() + "Y" + kp.getY()
						+ "Z" + kp.getDestX() + "W" + kp.getDestY() + "H"
						+ ((kp.isHeld() == true) ? "Y" : "N");
			}
		}

		if (msg != "") { // there are some new packages identified that need to
							// be broadcast to others
			get_agent_info(id).update_loc(this.x, this.y);
			// piggyback information to report where you are
			msg += "A" + this.id + "X" + this.x + "Y" + this.y;

			// piggyback information to report move history
			// String move_history_msg = construct_move_history_message();
			String move_history_msg = "M";
			msg += move_history_msg;
			next_action = new Say(msg);
			move_history.clear();
			return;
		}

		/*
		 * //if I have moved for more than half map_size, broadcast my move
		 * action_history to other team members //so that they can update their
		 * map information. if(move_history.size() > map_size/2){ String
		 * move_history_msg = construct_move_history_message();
		 * move_history.clear(); next_action = new Say(move_history_msg);
		 * return; }
		 */
		// the agent with the smallest ID is the coordinator
		int smallest = Integer.MAX_VALUE;
		for (int i = 0; i < agt_info.size(); ++i) {
			if (Integer.parseInt(agt_info.get(i).getId()) < smallest) {
				smallest = Integer.parseInt(agt_info.get(i).getId());
			}
		}

		coordinator = (Integer.parseInt(id) == smallest);

		if (last_action instanceof Dropoff) {
			// does my last drop-off action delivers the package into its
			// destination?
			int index_package_delivered = -1;
			for (int i = 0; i < percept.getVisPackages().length; ++i) {
				if (percept.getVisPackages()[i].getId() == dropped_package)
					index_package_delivered = i;
			}

			if (index_package_delivered < 0) { // I did not find the package,
												// i.e., the package has been
												// successfully delivered.
				map[get_package_info(dropped_package).getX()][get_package_info(
						dropped_package).getY()] = map_state.Clear;
				pkg_info.remove(get_package_info(dropped_package));

				msg = "D" + dropped_package + "P"; // delivered package
													// successfully.

			} else {
				map[get_package_info(dropped_package).getX()][get_package_info(
						dropped_package).getY()] = map_state.Clear;
				pkg_info.remove(get_package_info(dropped_package));
				pkg_info.add(new KnownPackage(
						percept.getVisPackages()[index_package_delivered]));
				map[percept.getVisPackages()[index_package_delivered].getX()][percept
						.getVisPackages()[index_package_delivered].getY()] = map_state.Package;

				msg = "D" + dropped_package;
				msg += "P"
						+ percept.getVisPackages()[index_package_delivered]
								.getId()
						+ "X"
						+ percept.getVisPackages()[index_package_delivered]
								.getX()
						+ "Y"
						+ percept.getVisPackages()[index_package_delivered]
								.getY()
						+ "Z"
						+ percept.getVisPackages()[index_package_delivered]
								.getDestX()
						+ "W"
						+ percept.getVisPackages()[index_package_delivered]
								.getDestY()
						+ "H"
						+ ((percept.getVisPackages()[index_package_delivered]
								.isHeld() == true) ? "Y" : "N");
			}

			get_agent_info(id).update_loc(this.x, this.y);
			get_agent_info(id).update_state(agent_state.IDLE);
			msg += "A" + this.id + "X" + this.x + "Y" + this.y; // tell others
																// who I am and
																// where I am.

			// piggyback information to report move history
			// String move_history_msg = construct_move_history_message();
			String move_history_msg = "M";
			msg += move_history_msg;

			move_history.clear();
			state = state_t.IDLE;
			next_action = new Say(msg);
			return;
		}

		if (last_action instanceof Pickup) {
			if (percept.getHeldPackage() != null) {
				KnownPackage intented_pkg = new KnownPackage(
						percept.getHeldPackage());

				map[intented_pkg.getX()][intented_pkg.getY()] = map_state.Clear;
				msg = "U" + picked_up_package;

				get_agent_info(id).update_loc(this.x, this.y);
				get_agent_info(id).update_state(agent_state.CARRY);
				msg += "A" + this.id + "X" + this.x + "Y" + this.y; // tell
																	// others
																	// who I am
																	// and where
																	// I am.

				// piggyback information to report move history
				// String move_history_msg = construct_move_history_message();
				String move_history_msg = "M";
				msg += move_history_msg;

				move_history.clear();
				state = state_t.CARRY;
				next_action = new Say(msg);
				return;
			} else {
				state = state_t.IDLE;
			}
		}
		
while (true) {
		// From now on, I don't need to report any external events.
		// If i am a coordinator and some event triggers global coordination,
		// then
		// I will compute how to coordinate my team mates, i.e., I will decide
		// how to map
		// tasks(package or exploration) to other available agents (i.e., agents
		// in either IDLE or ARRROACH or EXPLORE state)
		// After the task assignment is finished, I will broadcast to other
		// agents.
		if (coordinator && need_coordinate) { // I am the coordinator, so I need
												// to tell each idle agent
												// (i.e., has no package in
												// hand) what
			// package is right for it to achieve.
			ArrayList<KnownAgent> available_agts = new ArrayList<KnownAgent>();
			for (KnownAgent ka : agt_info) {
				if (ka.get_state() == KnownAgent.agent_state.INTENT
						|| ka.get_state() == KnownAgent.agent_state.IDLE) {
					available_agts.add(ka);
				}
			}
			ArrayList<KnownPackage> available_pkgs = new ArrayList<KnownPackage>();
			for (KnownPackage kp : pkg_info) {
				if (!kp.isHeld()) {
					available_pkgs.add(kp);
				}
			}

			Map<KnownAgent, KnownPackage> map = map_packages(available_agts,
					available_pkgs);
			String str = "C";
			for (KnownAgent aa : available_agts) {
				KnownPackage kp = map.get(aa);
				str += "A" + aa.getId();
				if (kp.getId() != -1) {
					str += "P" + kp.getId();
				} else
					str += "P" + "E";
			}

			need_coordinate = false;
			if(str != "C"){
				next_action = new Say(str);
				return;
			}else {
				continue;
			}
		}

		if (state == state_t.PICKUP){
			int dira = get_direction(new Location(this.x, this.y), goal);
			next_action = new Pickup(dira);
			return;
		}
		
		if(state == state_t.DROPOFF){
			VisiblePackage pkg_held = percept.getHeldPackage();
			if(pkg_held != null){
				int dira = get_direction(new Location(this.x, this.y),
											new Location(pkg_held.getDestX(), pkg_held.getDestY()));
				next_action = new Dropoff(dira);
				return;
			}else{
				state = state_t.IDLE;
				continue;
			}
		}
		
			if (state == state_t.IDLE) {
				plan.clear();
				
				local_intention = Find_Nearest_Package(new Location(this.x,
						this.y));
				if (local_intention != null){
					goal = local_intention;
					state = state_t.INTENT;
				}
				else
					state = state_t.EXPLORE;
				continue;
			} 
			// If am not holding some package, let's see if I at least have an
			// intention (i.e., target package)
			// if I have and coordinator does not drop my intention, I continue
			// to reach this intention
			// else if coordinator dropped my intention and has another
			// intention ready for me, I will drop my
			// subgoal related to the obsolete intention and go for the new
			// intention
			// if I do not have a intention, I will explore the world until the
			// time I am assigned intention.
			if (state == state_t.INTENT) {
				int dira = get_direction(new Location(this.x, this.y), goal);
				if (dira != -1) { // I am close to the package
					state = state_t.PICKUP;
					continue;
				}

				ArrayList<Integer> path = null;
				if (plan.size() <= 0) { // generate a plan
					for (int d = Integer.parseInt(id) % 4; d != (Integer
							.parseInt(id) + 3) % 4; d = (d + 1) % 4) {
						Point dest = new Point(goal.getX()
								+ Direction.DELTA_X[d], goal.getY()
								+ Direction.DELTA_Y[d], null);
						path = compute_path(dest, -1);
						if (path != null && path.size() > 0) {
							break;
						}
					}

					if (path == null || path.size() <= 0) {
						state = state_t.EXPLORE;
						continue;
					}
					plan = path;
				}

				// before I continue to go as my plan indicate, check the
				// current instant situation to see
				// if there is any moving obstacles(i.e., agents carrying or not
				// carrying a package) in front of me
				// so that I don't collide with them.
				// If there is something in front of me, I just wait and idle.
				Integer go = plan.get(0);
				if (check_movable(go, -1) == false) {
					Random r = new Random();
					while (true) {
						int planB = r.nextInt(4);
						if (check_movable(planB, -1)) {
							// invalidate plan
							plan.clear();
							// update location info
							this.x += Direction.DELTA_X[planB];
							this.y += Direction.DELTA_Y[planB];
							move_history.add(planB);
							state = state_t.IDLE;
							
							next_action = new Move(planB);
							return;
						}
					}
				}

				plan.remove(0);
				this.x += Direction.DELTA_X[go];
				this.y += Direction.DELTA_Y[go];
				move_history.add(go);
				next_action = new Move(go);
				return;
			}

			// I am holding some package, so I need to deliver it to its
			// destination no matter what
			// I may still need to achieve several subgoals before I finally
			// reach the delivery position
			if (state == state_t.CARRY) {
				VisiblePackage pkg_held = percept.getHeldPackage();
				System.out.println("agent" + id + " is carrying pkg_held: "
						+ pkg_held);

				if (pkg_held == null) {
					state = state_t.IDLE;
					continue;
				}

				int pkg_dir = get_direction(cur, new Location(pkg_held.getX(),
						pkg_held.getY()));

				// check to see if the delivery point is clear (having a static
				// package that is not in my hand)

				// check to see if there is some other agent in the delivery
				// check to see if I am ready to drop the package.
				dest = new Location(pkg_held.getDestX(), pkg_held.getDestY());
				int dira = get_direction(new Location(this.x, this.y), dest);
				if (dira != -1) { // I am close to the delivery position
					dropped_package = pkg_held.getId();
					state = state_t.DROPOFF;
					continue;
				}

				ArrayList<Integer> path = null;
				if (plan.size() <= 0) { // generate a plan
					for (int d = Integer.parseInt(id) % 4; d != (Integer
							.parseInt(id) + 3) % 4; d = (d + 1) % 4) {
						Point dest = new Point(pkg_held.getDestX()
								+ Direction.DELTA_X[d], pkg_held.getDestY()
								+ Direction.DELTA_Y[d], null);
						path = compute_path(dest, pkg_dir);
						if (path != null && path.size() > 0) {
							break;
						}
					}
					if (path == null || path.size() <= 0) {
						state = state_t.EXPLORE;
						continue;
					}

					plan = path;
				}

				// before I continue to go as my plan indicate, check the
				// current instant situation to see
				// if there is any moving obstacles(i.e., agents carrying or not
				// carrying a package) in front of me
				// so that I don't collide with them.
				// If there is something in front of me, I just wait and idle.
				Integer go = plan.get(0);

				if (check_movable(go, pkg_dir) == false) {
					Random r = new Random();
					while (true) {
						int planB = r.nextInt(4);
						if (check_movable(planB, pkg_dir)) {
							// invalidate plan
							plan.clear();
							// update location info
							this.x += Direction.DELTA_X[planB];
							this.y += Direction.DELTA_Y[planB];
							move_history.add(planB);
							state = state_t.IDLE;
							
							next_action = new Move(planB);
							return;
						}
					}
				}

				plan.remove(0);
				this.x += Direction.DELTA_X[go];
				this.y += Direction.DELTA_Y[go];
				move_history.add(go);
				next_action = new Move(go);
				return;
			}// end carry state

			if (state == state_t.EXPLORE) {
				Location goal = null;
				ArrayList<Integer> path = null;

				// we don't have a plan yet, generate a plan
				if (plan == null || plan.size() <= 0) {
					// if I did have a goal, is the goal reachable now?
					if (global_intention >= 0 || local_intention != null) {
						VisiblePackage pkg_held = percept.getHeldPackage();

						if (pkg_held == null) { // I am heading to the package,
												// not holding it
							if (global_intention >= 0)
								goal = new Location(get_package_info(
										global_intention).getX(), get_package_info(
										global_intention).getY());
							else
								goal = local_intention;
							
							path = compute_path(new Point(goal, null), -1);
							
							// the goal is still not reachable
							if (path == null || path.size() <= 0) {
								// Find closest Unknown area using BFS
								Location nearest_clear = Find_Nearest_Clear(goal);
								goal = nearest_clear;
								
								path = compute_path(new Point(goal, null), -1);
							}else {
								state = state_t.INTENT;
								continue;
							}
							
							if (path == null || path.size() <= 0) {
								global_intention = -1;
								local_intention = Find_Nearest_Package(cur);
								state = state_t.IDLE;
								continue;
							}
							
						} else {
							int pkg_dir = get_direction(cur, new Location(
									pkg_held.getX(), pkg_held.getY()));
							dest = new Location(pkg_held.getDestX(),
									pkg_held.getDestY());
							path = compute_path(new Point(dest, null), pkg_dir);
							
							// the goal is still not reachable
							if (path == null || path.size() <= 0) {
								// Find closest Unknown area using BFS
								Location nearest_clear = Find_Nearest_Clear(dest);
								dest = nearest_clear;
	
								path = compute_path(new Point(dest, null),
										pkg_dir);	
							} else {
								state = state_t.CARRY;
								continue;
							}
							
							if (path == null || path.size() <= 0) {
								global_intention = -1;
								Random r = new Random();
								VisiblePackage vp = percept.getVisPackages()[r.nextInt(percept.getVisPackages().length)];
								local_intention = new Location(vp.getX(), vp.getY());
								state = state_t.INTENT;
								dropped_package = pkg_held.getId();
								next_action = new Dropoff(pkg_dir);
								return;
							}
						}

					} else { // have no intention at all.
						// the goal is still not reachable
						if (path == null || path.size() <= 0) {
							// Find closest Unknown area using BFS
							Location unknown = Find_Nearest_Unknown(cur);
							if (unknown == null) {
								state = state_t.IDLE;
								continue;
							}
							// there exists unknown areas
							Location nearest_clear = Find_Nearest_Clear(unknown);
							goal = nearest_clear;
							path = compute_path(new Point(goal, null), -1);
						}
						
						if (path == null || path.size() <= 0) {
							global_intention = -1;
							local_intention = Find_Nearest_Package(cur);
							state = state_t.IDLE;
							continue;
						}
					}

					if (path == null || path.size() <= 0) {
						state = state_t.IDLE;
						continue;
					}

					plan = path;
				}

				// get the first plan
				Integer go = plan.get(0);

				// collision detection and solution
				VisiblePackage pkg_held = percept.getHeldPackage();
				if (pkg_held == null) {
					if (check_movable(go, -1) == false) {
						Random r = new Random();
						while (true) {
							int planB = r.nextInt(4);
							if (check_movable(planB, -1)) {
								// invalidate plan
								plan.clear();
								// update location info
								this.x += Direction.DELTA_X[planB];
								this.y += Direction.DELTA_Y[planB];
								move_history.add(planB);
								state = state_t.IDLE;
								
								next_action = new Move(planB);
								return;
							}
						}
					}
				} else {
					int pkg_dir = get_direction(cur,
							new Location(pkg_held.getX(), pkg_held.getY()));
					if (check_movable(go, pkg_dir) == false) {
						Random r = new Random();
						while (true) {
							int planB = r.nextInt(4);
							if (check_movable(planB, pkg_dir)) {
								// invalidate plan
								plan.clear();
								// update location info
								this.x += Direction.DELTA_X[planB];
								this.y += Direction.DELTA_Y[planB];
								move_history.add(planB);

								next_action = new Move(planB);
								return;
							}
						}
					}
				}

				plan.remove(0);
				this.x += Direction.DELTA_X[go];
				this.y += Direction.DELTA_Y[go];
				move_history.add(go);
				next_action = new Move(go);
				return;
			}

		} //while

	}

	/**
	 * Have the agent select its next action to perform. Implements the action0:
	 * Per -> Ac function or the action: I -> Ac function, depending on whether
	 * or not the agent has internal state. Note, Per (or I) is not passed as a
	 * parameter, because we assume this information is recorded in an instance
	 * variable by the see() method.
	 */
	public Action selectAction() {
		System.out.println("-------------------------");
		printInfo();
		System.out.println("Action: " + next_action);
		System.out.println("-------------------------");
		System.out.println("-------------------------");
		last_action = next_action;
		return next_action;
	}

	/**
	 * Return a unique string that identifies the agent. This is particularly
	 * useful in multi-agent environments.
	 */
	public String getId() {
		return id;
	}

	public void printInfo() {
		System.out.println("Agents Known:");
		for (int i = 0; i < agt_info.size(); i++)
			System.out.println(agt_info.get(i));
		System.out.println("Package Known:");
		for (int i = 0; i < pkg_info.size(); i++)
			System.out.println(pkg_info.get(i));
		System.out.println("Map as far as I known:");
		for (int j = 0; j < map_size; ++j) {
			for (int i = 0; i < map_size; ++i) {
				if (map[i][j] == map_state.Unknown)
					System.out.print("*");
				else if (map[i][j] == map_state.Package)
					System.out.print("#");
				else if (map[i][j] == map_state.Clear)
					System.out.print("_");
				else
					assert (false);
			}
			System.out.println();
		}
	}

	public boolean in_map_range(Point p) {
		return p.getX() >= 0 && p.getX() < map_size && p.getY() >= 0
				&& p.getY() < map_size;
	}

}
